#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include "std_msgs/Float64.h"
#include "nav_msgs/GetMap.h"
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"
#include "message_filters/subscriber.h"
#include "tf/message_filter.h"

extern "C"{
#include "TinySLAM.h"
}

#define METERS_TO_MM    1000
#define MM_TO_METERS    0.001

#include <boost/thread.hpp>

//this class provides a tinyslam to ros adapter
//for reference see:
//tinySLAM: A SLAM algorithm in less than 200 lines C-language program by B. Steux
class TinySlam
{
  public:
    TinySlam();
    ~TinySlam();

    void publishTransform();
  
    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
    bool mapCallback(nav_msgs::GetMap::Request  &req,
                     nav_msgs::GetMap::Response &res);
    void publishLoop(double transform_publish_period);

  private:
    //ts instances
    ts_map_t m_ts_map;
    ts_state_t m_state;
    ts_position_t m_position;
    ts_position_t m_prev_odom;
    ts_laser_parameters_t m_lparams;

	//communication
    ros::NodeHandle m_node;
    ros::Publisher m_sst;
    ros::Publisher m_sstm;
    ros::ServiceServer m_ss;
    tf::TransformListener m_tf;
    message_filters::Subscriber<sensor_msgs::LaserScan>* m_scan_filter_sub;
    tf::MessageFilter<sensor_msgs::LaserScan>* m_scan_filter;
    tf::TransformBroadcaster* tfB_;
	
	nav_msgs::GetMap::Response m_map;
    ros::Duration m_map_update_interval;
    tf::Transform m_map_to_odom;
    
	
	// checks
    bool m_got_first_scan;
    bool m_got_map;    
	
	//avoid threading problems
	boost::mutex map_to_odom_mutex_;
    boost::mutex map_mutex_;

    int m_laser_count;
    
    boost::thread* transform_thread_;

    std::string m_base_frame;
    std::string m_laser_frame;
    std::string m_map_frame;
    std::string m_odom_frame;

    void updateMap();
    bool getOdomPose(ts_position_t& ts_pose, const ros::Time &t);
    bool initMapper(const sensor_msgs::LaserScan& scan);
    bool addScan(const sensor_msgs::LaserScan& scan, ts_position_t& pose);

    // parameters for tinyslam
    double m_sigma_xy;
    double m_sigma_theta;
    double m_hole_width;
    int m_span;
    double m_delta;

};
